﻿/*
	@Author: Andrew Erridge
*/
#include "PartRobotAgent.h"
#include "ScopedLock.h"
#include "Kit.h"
#include "GUIPartRobot.h"
#include <algorithm> //not used atm, should be

using namespace std;

PartRobotAgent::PartRobotAgent(string name)
	: Agent(name)
{
	partsDumpedSema = Sema(0);
	partRetrievedSema = Sema(0);
	partsInHands = std::vector<PartType>(4);
	tableStatus = INACCESSIBLE;


	//four hands, all empty right now
	for ( int i = 0; i < 4; ++i )
	{
		this->partsInHands.at( i ) = PartType::EMPTY;
	}
}

// Non-norms
void PartRobotAgent::handleBreak() {
	isBroken = true;
	((GUIPartRobot*) gui)->DoBreak();
	if ( partsInHands.at( 0 ) != PartType::EMPTY )
	{
		partTypesNeeded.push_back( partsInHands.at( 0 ) );
	}
	
	
	std::vector<PartType> oldTypesNeeded;

	for ( int i = 0; i < partTypesNeeded.size(); ++i )
	{
		oldTypesNeeded.push_back( partTypesNeeded.at( i ) );
		partTypesNeeded.at( i ) = myNests.at( 0 ).partTypeHeld;
	}

	while ( this->breakSemaphore.getValue() < 1 )
	{
		this->pickAndExecuteAnAction();
	}

	for ( int i = 0; i < partTypesNeeded.size(); ++i )
	{
		if ( !oldTypesNeeded.empty() ) 
		{
			partTypesNeeded.at( i ) = oldTypesNeeded.at( i );
		}
	}
}
void PartRobotAgent::handleFix() {
	isBroken = false;
	((GUIPartRobot*) gui)->DoFix();
}
// ------------------------------------------------------------------------------------


/*** MESSAGES ***/


/**
 * @fn	void PartRobotAgent::msgPartsDumped()
 *
 * @brief	Message parts dumped.
 *
 * @author	Dru
 * @date	11/5/2011
 */

void PartRobotAgent::msgPartsDumped()
{
	print("Received msgPartsDumped from my GUI");

	this->partsDumpedSema.post();
	this->stateChanged(); //necessary?
}

/**
 * @fn	void PartRobotAgent::msgPartRetrieved(int handLocation, PartType type)
 *
 * @brief	Message part retrieved.
 *
 * @author	Dru
 * @date	11/7/2011
 *
 * @param	handLocation	The hand location.
 * @param	type			The type.
 */
void PartRobotAgent::msgPartRetrieved(int handLocation, PartType type)
{
	print("msgPartRetrieved from my GUI");

	this->partRetrievedSema.post();
	this->stateChanged(); //necessary?
}

/**
 * @fn	void PartRobotAgent::msgImStable(Nest *nest)
 *
 * @brief	Message im stable.
 *
 * @author	Dru
 * @date	11/5/2011
 *
 * @param [in,out]	nest	If non-null, the nest.
 */
void PartRobotAgent::msgImStable(Nest *nest) 
{
	print("received msgImStable");
	{
		ScopedLock scopedPartsNeededListLock (&partsTypesNeededLock);

		//must it be scope-locked? // doesn't change any elements (probably no)
		for ( std::vector<MyNest>::iterator it = myNests.begin(); it != myNests.end(); it++ )
		{
			if ( (*it).nest == nest ) 
			{
				(*it).state = STABLE;
			}
		}
	}
	this->stateChanged();
}

/**
 * @fn	void PartRobotAgent::msgMakeKit(Kit *kit)
 *
 * @brief	Message make kit.
 *
 * @author	Dru
 * @date	11/5/2011
 *
 * @param [in,out]	kit	If non-null, the kit.
 */
void PartRobotAgent::msgMakeKit(KitInterface *kit)
{
	print("Received msgMakeKit");
	if( kit == NULL )
	{
		print("received NULL kit");
	}
	else {

		if ( kit->getPartTypesNeeded().size() == 0 )
		{
			print("kit received is empty (needs no parts)");
		}	

		MyKit myKit;
		myKit.kit = kit;
		myKit.state = WAITING;
		this->myKits.push_back(myKit);
		this->stateChanged();
	}
}

/**
 * @fn	void PartRobotAgent::msgYouCanGoToTable()
 *
 * @brief	Message you can go to table.
 *
 * @author	Dru
 * @date	11/5/2011
 */
void PartRobotAgent::msgYouCanGoToTable()
{
	print("Received msgYouCanGoToTable()");

	tableStatus = ACCESSIBLE;

	this->stateChanged();
}


/*** HELPERS ***/

bool PartRobotAgent::handsAreEmpty()
{
	for ( int i = 0; i < this->partsInHands.size(); ++i )
	{
		if ( partsInHands.at( i ) != PartType::EMPTY )
		{
			return false;
		}
	}
	return true;
}

/**
 * @fn	bool PartRobotAgent::isPartTypeBeingMade( PartType pt )
 *
 * @brief	Query if 'pt' is part type being made.
 *
 * @author	Dru
 * @date	11/9/2011
 *
 * @param	pt	The part type.
 *
 * @return	true if part type being made, false if not.
 */

bool PartRobotAgent::isPartTypeBeingMade( PartType pt )
{
	for ( int i = 0; i < myNests.size(); ++i )
	{
		if ( myNests.at( i ).partTypeHeld == pt )
		{
			return true;
		}
	}

	return false;
}


bool PartRobotAgent::kitsAreInProgress()
{
	for ( int i = 0; i < myKits.size(); ++i )
	{
		if ( myKits.at( i ).state == IN_PROGRESS)
		{
			return true;
		}
	}
	return false;
}

/**
 * @fn	bool PartRobotAgent::handsAreFull()
 *
 * @brief	Gets the hands are full.
 *
 * @author	Dru
 * @date	11/7/2011
 *
 * @return	true if it succeeds, false if it fails.
 */
bool PartRobotAgent::handsAreFull()
{
	for ( int i = 0; i < partsInHands.size(); ++i )
	{
		if ( partsInHands.at( i ) == PartType::EMPTY )
		{
			return false;
		}
	}
	return true;
}

/*** ACTIONS ***/

/**
 * @fn	void PartRobotAgent::pickUp(PartType pt, int freeHandLocation, Nest *n)
 *
 * @brief	Pick up.
 *
 * @author	Dru
 * @date	11/7/2011
 *
 * @param	pt					The point.
 * @param	freeHandLocation	The free hand location.
 * @param [in,out]	n			If non-null, the n.
 */
void PartRobotAgent::pickUp(PartType pt, int freeHandLocation, Nest *n)
{
	if (this->gui != NULL)
	{
		this->gui->doPickup( n->getGUI(), freeHandLocation );
		this->Do("doPickUp()");
		//wait on sempahore
		this->partRetrievedSema.wait();
	}
	else
	{
		this->Do("GUI = NULL: doPickup()");
	}

	partsInHands.at(freeHandLocation) = pt; // caused errors before, might be fixed

	{
		ScopedLock scopedPartsNeededListLock (&partsTypesNeededLock);
		std::vector<PartType>::iterator eraseMe;
		for (std::vector<PartType>::iterator it = partTypesNeeded.begin(); it != partTypesNeeded.end(); ++it)
		{
			if ( (*it) == pt )
			{
				eraseMe = it;
			}
		}
		partTypesNeeded.erase( eraseMe ); //only delete one part
	}
}

/**
 * @fn	void PartRobotAgent::askToDumpLoad()
 *
 * @brief	Ask to dump load.
 *
 * @author	Dru
 * @date	11/5/2011
 */
void PartRobotAgent::askToDumpLoad()
{
	print("Asking KitRobot CanIGoToTable()");
	
	kitRobot->msgCanIGoToTable();
}

/**
 * @fn	void PartRobotAgent::dumpLoad()
 *
 * @brief	Dumps the load.
 *
 * @author	Dru
 * @date	11/5/2011
 */
void PartRobotAgent::dumpLoad()
{
	MyKit* kitToReceiveParts;
	
	for (int i = 0; i < 2; ++i) 
	{
		for (int j = 0; j < myKits.size(); ++j)
		{
			if ( (myKits.at( j ).state == IN_PROGRESS) && (myKits.at( j ).kit == sharedDataTable->peek( i )) )
			{
				kitToReceiveParts = &myKits.at( j );
				Kit* guiKit = static_cast<Kit *>( kitToReceiveParts->kit );
				if (this->gui != NULL)
				{
					this->gui->doDumpLoad( guiKit );
					this->Do("doDumpLoad()");
					//wait, acquire a semaphore
					this->partsDumpedSema.wait();
				}
				else
				{
					this->Do("GUI == NULL: doDumpLoad()");
				}
			}
		}
	}

	for ( int i = 0; i < 4; ++i )
	{
		this->partsInHands.at( i ) = EMPTY;
	}

	tableStatus = INACCESSIBLE;
	print("Telling kitRobot doneDumpingParts");
	kitRobot->msgDoneDumpingParts();	

	if ( kitToReceiveParts->kit->isFull()) {
		Kit* kitRobotsKit = static_cast<Kit *>(kitToReceiveParts->kit);
		print("Telling kit robot KitFinished");
		kitRobot->msgKitFinished( kitRobotsKit );
		kitToReceiveParts->state = FINISHED;
		partTypesNeeded.clear();
	}
}

/**
 * @fn	void PartRobotAgent::allocatePartsToNests(Kit *kit)
 *
 * @brief	Allocate parts to nests.
 *
 * @author	Dru
 * @date	11/5/2011
 *
 * @param [in,out]	kit	If non-null, the kit.
 */
void PartRobotAgent::allocatePartsNeededToNests(KitInterface *kit)
{
	for (int i = 0; i < kit->getPartTypesNeeded().size(); ++i )
	{
		if ( kit->getPartTypesNeeded().at( i ) != PartType::EMPTY ) 
		{
			if ( myNests.at( i ).partTypeHeld != kit->getPartTypesNeeded().at( i ))
			{
				myNests.at( i ).isMakingUsefulPart = true;
				myNests.at( i ).partTypeHeld = kit->getPartTypesNeeded().at( i );
				myNests.at( i ).nest->msgPleaseGetParts( kit->getPartTypesNeeded().at( i ) );
				myNests.at( i ).state = UNSTABLE;
				myNests.at( i ).approxPartsIncoming = 20;
			}
			else if ( myNests.at( i ).approxPartsIncoming < 12 )
			{
				myNests.at( i ).isMakingUsefulPart = true;
				myNests.at( i ).partTypeHeld = kit->getPartTypesNeeded().at( i );
				myNests.at( i ).nest->msgPleaseGetParts( kit->getPartTypesNeeded().at( i ) );
				myNests.at( i ).state = UNSTABLE; //maybe is fucking shit up? doesn't purge.
				myNests.at( i ).approxPartsIncoming += 20;
			}
		}
	}

	//if one of two nests has not been assigned a type, assign it one
	for ( int i = 0; i < myNests.size(); i += 2 )
	{
		if ( myNests.at( i ).partTypeHeld == PartType::EMPTY ^ myNests.at( i + 1 ).partTypeHeld == PartType::EMPTY )
		{
			if ( myNests.at( i ).partTypeHeld == PartType::EMPTY )
			{
				myNests.at( i ).isMakingUsefulPart = true;
				myNests.at( i ).partTypeHeld = myNests.at( i + 1 ).partTypeHeld;
				myNests.at( i ).nest->msgPleaseGetParts( myNests.at( i + 1 ).partTypeHeld );
				myNests.at( i ).state = UNSTABLE;
				myNests.at( i ).approxPartsIncoming = 20;
			}
			else
			{
				myNests.at( i + 1 ).isMakingUsefulPart = true;
				myNests.at( i + 1 ).partTypeHeld = myNests.at( i ).partTypeHeld;
				myNests.at( i + 1 ).nest->msgPleaseGetParts( myNests.at( i ).partTypeHeld );
				myNests.at( i + 1 ).state = UNSTABLE; 
				myNests.at( i + 1 ).approxPartsIncoming += 20;
			}
		}
	}
}

/**
 * @fn	bool PartRobotAgent::pickAndExecuteAnAction()
 *
 * @brief	Gets the pick and execute an action.
 *
 * @author	Dru
 * @date	11/5/2011
 *
 * @return	true if it succeeds, false if it fails.
 */
bool PartRobotAgent::pickAndExecuteAnAction()
{
	// SCHEDULER RULE # 1
	//if your hands are full, ask to dump your load or dump it
	if ( handsAreFull() )
	{
		if ( tableStatus == TableStatus::ACCESSIBLE ) 
		{
			this->dumpLoad();
			tableStatus = TableStatus::INACCESSIBLE;
			return true;
		}
		else if ( tableStatus == TableStatus::INACCESSIBLE )
		{
			this->askToDumpLoad();
			tableStatus = TableStatus::PENDING;
			return true;
		}
	}
	
	// SCHEDULER RULE # 2
	// if there are no kits in progress, but one waiting, set it in progress 
	// and allocate parts to their nests so you can begin working on it
	if ( !kitsAreInProgress() ) 
	{
		if ( !isBroken )
		{
			for ( int i = 0; i < myKits.size(); ++i )
			{
				if ( myKits.at( i ).state == WAITING ) 
				{
					MyKit* myKitToWorkOn = &myKits.at( i );
					myKitToWorkOn->state = IN_PROGRESS;

					//my partTypesNeeded are now the same as the kit's
					this->partTypesNeeded.clear();
					for ( int j = 0; j < myKitToWorkOn->kit->getPartTypesNeeded().size(); ++j )
					{
						//this could break things
						if ( myKitToWorkOn->kit->getPartTypesNeeded().at( j ) != PartType::EMPTY )
						{
							this->partTypesNeeded.push_back( myKitToWorkOn->kit->getPartTypesNeeded().at( j ) );
						}
					}

					//make sure the nests have the right part types so I can fill the kit
					this->allocatePartsNeededToNests( myKitToWorkOn->kit ); 
				
					return true;
				}
			}
		}
	}

	// SCHEDULER RULE # 3
	//if a nest is stable and has a part you need, and you have a free hand, pick it up
	if ( partTypesNeeded.size() != 0) 
	{
		for ( int i = 0; i < myNests.size(); ++i )
		{
			if ( myNests.at( i ).state == STABLE ) 
			{
				for ( int j = 0; j < partTypesNeeded.size(); ++j )
				{
					if ( myNests.at( i ).partTypeHeld == partTypesNeeded.at( j ) )
					{
						for ( int k = 0; k < partsInHands.size(); ++k )
						{
							if ( partsInHands.at( k ) == PartType::EMPTY )
							{
								this->pickUp( myNests.at( i ).partTypeHeld, k, myNests.at( i ).nest );
								
								myNests.at( i ).nest->msgPartWasTaken();

								myNests.at( i ).state = UNSTABLE;

								myNests.at( i ).approxPartsIncoming -= 1;

								return true;
							}
						}
					}
				}
			}
		}
	}
	else 
	{
		//print("partTypesNeeded = 0");
	}

	// SCHEDULER RULE # 4
	//if one of two nests has not been assigned a type, assign it one
	for ( int i = 0; i < myNests.size(); i += 2 )
	{
		if ( myNests.at( i ).partTypeHeld == PartType::EMPTY ^ myNests.at( i + 1 ).partTypeHeld == PartType::EMPTY )
		{
			if ( myNests.at( i ).partTypeHeld == PartType::EMPTY )
			{
				myNests.at( i ).isMakingUsefulPart = true;
				myNests.at( i ).partTypeHeld = myNests.at( i + 1 ).partTypeHeld;
				myNests.at( i ).nest->msgPleaseGetParts( myNests.at( i + 1 ).partTypeHeld );
				myNests.at( i ).state = UNSTABLE;
				myNests.at( i ).approxPartsIncoming = 20;
			}
			else
			{
				myNests.at( i + 1 ).isMakingUsefulPart = true;
				myNests.at( i + 1 ).partTypeHeld = myNests.at( i ).partTypeHeld;
				myNests.at( i + 1 ).nest->msgPleaseGetParts( myNests.at( i ).partTypeHeld );
				myNests.at( i + 1 ).state = UNSTABLE;
				myNests.at( i + 1 ).approxPartsIncoming = 20;
			}
		}
	}

	// SCHEDULER RULE # 5
	//if you've nothing else to do, and you have a part in hand, dump it.
	if ( !handsAreEmpty() )
	{
		if ( tableStatus == TableStatus::ACCESSIBLE ) 
		{
			this->dumpLoad();
			tableStatus = TableStatus::INACCESSIBLE;
			return true;
		}
		else if ( tableStatus == TableStatus::INACCESSIBLE )
		{
			this->askToDumpLoad();
			tableStatus = TableStatus::PENDING;
			return true;
		}
	}
	
	return false;
}


/*** GETTERS AND SETTERS ***/


/**
 * @fn	void PartRobotAgent::addNest(Nest *nest)
 *
 * @brief	Adds a nest.
 *
 * @author	Dru
 *
 * @param [in,out]	nest	If non-null, the nest.
 */
void PartRobotAgent::addNest( Nest *nest )
{
	MyNest myNest;
	myNest.nest = nest;
	myNest.state = UNSTABLE;
	myNest.isMakingUsefulPart = false;
	myNest.partTypeHeld = EMPTY;
	myNest.approxPartsIncoming = 0;
	this->myNests.push_back( myNest );
}

/**
 * @fn	GUIPartRobot* PartRobotAgent::getGUI()
 *
 * @brief	Gets the graphical user interface.
 *
 * @author	Dru
 *
 * @return	null if it fails, else the graphical user interface.
 */
GUIPartRobot* PartRobotAgent::getGUI(void)
{
	return (static_cast<GUIPartRobot*>(this->gui));
}

/**
 * @fn	void PartRobotAgent::setGUI(GUIPartRobot* guiPR)
 *
 * @brief	Sets a graphical user interface.
 *
 * @author	Dru
 * @date	11/7/2011
 *
 * @param [in,out]	guiPR	If non-null, the graphical user interface pr.
 */

void PartRobotAgent::setGUI(GUIPartRobotInterface* guiPR)
{
	this->gui = guiPR;
}


/**
 * @fn	void PartRobotAgent::setTable(Table *table)
 *
 * @brief	Sets a table.
 *
 * @author	Dru
 *
 * @param [in,out]	table	If non-null, the table.
 */
void PartRobotAgent::setTable(TableInterface *table)
{
	this->sharedDataTable = table;
}

/**
 * @fn	void PartRobotAgent::setKitRobot(KitRobot *kitRobot)
 *
 * @brief	Sets a kit robot.
 *
 * @author	Dru
 *
 * @param [in,out]	kitRobot	If non-null, the kit robot.
 */
void PartRobotAgent::setKitRobot(KitRobot *kitRobot)
{
	this->kitRobot = kitRobot;
}

/**
 * @fn	std::string PartRobotAgent::toString() const
 *
 * @brief	Convert this object into a string representation.
 *
 * @author	Dru
 * @date	11/5/2011
 *
 * @return	A string representation of this object.
 */
std::string PartRobotAgent::toString() const 
{
	return "Part Robot " + getName(); 
}


PartRobotAgent::~PartRobotAgent(void)
{
	//TODO DELETE YOUR CRAP
}